/*---------------------------------------------------------------------
 * File name: ulink.cpp
 *
 * Copyright (c) <2020-2022>, <ChenLong>
 *
 * All rights reserved.
 *
 * Author: ChenLong
 * Email: worldlong@foxmail.com
 *--------------------------------------------------------------------*/
#include "ulink.h"
#include <string.h>
/*
*/
uint8_t ULink::check_calculate()
{
  uint8_t check = msg.head + (uint8_t)msg.id + (uint8_t)(msg.id >> 8) + msg.len;
  for(int i = 0; i < _count; i++) {
    check += msg.data[i];
  }
  return check;
}
/*
*/
bool ULink::check()
{
  return (check_calculate() == msg.check);
}
/*
*/ 
bool ULink::parse(uint8_t data)
{ 
  switch (_state)
  {
  case STATE_HEAD:
    if (data == _head) {
      msg.head = _head;
      _state = STATE_LEN;
    }
    break;
  case STATE_LEN:
    msg.len = data;
    _state = STATE_ID_LSB;
    break;
  case STATE_ID_LSB: 
    msg.id = data;
    _state = STATE_ID_MSB;
    break;
  case STATE_ID_MSB: 
    msg.id += data << 8;
    _count = 0;
    if (msg.len > ULINK_MSG_SIZE_MIN) {
      _state = STATE_DATA;
    } else {
      _state = STATE_CHECK;
    }
    break;
  case STATE_DATA:
    msg.data[_count++] = data;
    if (_count == msg.len - ULINK_MSG_SIZE_MIN) {
      _state = STATE_CHECK;
    }
    break;
  case STATE_CHECK:
    msg.check = data;
    _state = STATE_HEAD;
    return check();
  }
  return false;
}
/*
*/
int ULink::packet(uint8_t* buff)
{
  buff[0] = msg.head;
  buff[1] = msg.len;
  buff[2] = msg.id;
  buff[3] = msg.id >> 8;
  memcpy(buff + 4, msg.data, _count);
  buff[msg.len - 1] = msg.check;
  return msg.len;
}
/*
*/
void ULink::build_start(uint16_t id)
{
  msg.len = ULINK_MSG_SIZE_MIN;
  msg.id = id;
  _count = 0;
}
/*
 */
void ULink::build_end()
{
  msg.len = _count + ULINK_MSG_SIZE_MIN;
  msg.check = check_calculate();
}
/*
 */
void ULink::insert_u8(uint8_t data)
{
  msg.data[_count++] = data;
}
/*
 Little-Endian
 */
void ULink::insert_u16(uint16_t data)
{
  msg.data[_count++] = (uint8_t)data;
  msg.data[_count++] = (uint8_t)(data >> 8);
}
/*
Little-Endian
 */
void ULink::insert_u24(uint32_t data)
{
  msg.data[_count++] = (uint8_t)data;
  msg.data[_count++] = (uint8_t)(data >> 8);
  msg.data[_count++] = (uint8_t)(data >> 16);
}
/*
Little-Endian
 */
void ULink::insert_u32(uint32_t data)
{
  msg.data[_count++] = (uint8_t)data;
  msg.data[_count++] = (uint8_t)(data >> 8);
  msg.data[_count++] = (uint8_t)(data >> 16);
  msg.data[_count++] = (uint8_t)(data >> 24);
}
/*
Little-Endian
 */
void ULink::insert_f32(float data)
{
  uint32_t tmp;
  tmp = *((uint32_t*)(&data));
  insert_u32(tmp);
}
/*
 */
void ULink::insert_bytes(uint8_t *buffer, int number)
{
  memcpy(msg.data + _count, buffer, number);
  _count += number;
}
/*
 */
void ULink::insert_string(char *str)
{
  strcpy((char*)(&msg.data[_count]), str);
  _count += strlen(str) + 1;
}


/*
Little-Endian
 */
uint8_t ULink::extract_u8(int index)
{
  return msg.data[index];
}

/*
Little-Endian
 */
uint16_t ULink::extract_u16(int index)
{
  return (uint16_t)(msg.data[index] + (msg.data[index + 1] << 8));
}

/*
Little-Endian
 */
uint32_t ULink::extract_u24(int index)
{
  return (uint32_t)(msg.data[index] + (msg.data[index + 1] << 8) + (msg.data[index + 2] << 16));
}

/*
Little-Endian
 */
uint32_t ULink::extract_u32(int index)
{
  return (uint32_t)(msg.data[index] + (msg.data[index + 1] << 8) + (msg.data[index + 2] << 16) + (msg.data[index + 3] << 24));
}

/*
Little-Endian
 */
float ULink::extract_f32(int index)
{
  uint32_t tmp = extract_u32(index);
  return *(float*)(&tmp);
}

/*
Little-Endian
 */
uint32_t ULink::extract_bytes(uint8_t *buffer, int number, int index)
{
  memcpy(buffer, &msg.data[index], number);
  return number;
}

/*
Little-Endian
 */
uint32_t ULink::extract_string(char *str, int index)
{
  strcpy(str, (char const*)(&msg.data[index]));
  return strlen(str) + 1;
}



